from geometry_msgs.msg import PoseStamped,Pose
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult
from tf2_ros import TransformListener, Buffer #坐标监听器
from tf_transformations import euler_from_quaternion, quaternion_from_euler #四元数转欧拉角函数
import rclpy.time
from rclpy.node import Node
from autopartol_interfaces.srv import SpeechText
import math
from sensor_msgs.msg import Image # 消息接口
from cv_bridge import CvBridge # 转换图像格式
import cv2 # 保存图像


class PartolNode(BasicNavigator):
    def __init__(self, node_name='partol_node'):
        super().__init__(node_name)
        self.declare_parameter('initial_point', [0.0,0.0,0.0])
        self.declare_parameter('target_points', [0.0,0.0,0.0 ,1.0,1.0, 1.57])
        self.declare_parameter('img_save_path','')
        self.initial_point = self.get_parameter('initial_point',).value
        self.target_points = self.get_parameter('target_points',).value
        self.img_save_path = self.get_parameter('img_save_path',).value
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.speech_client = self.create_client(SpeechText, 'speech_text')
        self.cv_bridge = CvBridge()
        self.lastest_img = None
        self.img_sub = self.create_subscription(Image, '/camera_sensor/image_raw', self.img_callback, 1)
        
    def img_callback(self, msg):
        self.lastest_img = msg
    def record_img(self):
        if self.lastest_img is not None:
            pose = self.get_current_pose()
            cv_image = self.cv_bridge.imgmsg_to_cv2(self.lastest_img)
            cv2.imwrite(
                f'{self.img_save_path}img_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png',
                cv_image
            )
        
    def get_pose_by_xyyaw(self,x,y,yaw):
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.pose.position.x = x
        pose.pose.position.y = y
        qua = quaternion_from_euler(0,0,yaw)
        pose.pose.orientation.x = qua[0]
        pose.pose.orientation.y = qua[1]
        pose.pose.orientation.z = qua[2]
        pose.pose.orientation.w = qua[3]
        return pose
        
    def init_robot_pose(self):
        self.initial_point = self.get_parameter('initial_point').value
        init_pose = self.get_pose_by_xyyaw(self.initial_point[0], self.initial_point[1], self.initial_point[2])
        self.setInitialPose(init_pose)
        self.waitUntilNav2Active()
        
    def get_target_point(self):
        points = []
        self.target_points = self.get_parameter('target_points').value
        for index in range(int(len(self.target_points)/3)):
            x = self.target_points[index*3]
            y = self.target_points[index*3+1]
            yaw = self.target_points[index*3+2]
            points.append([x,y,yaw])
            self.get_logger().info(f'获取到目标点{index}->{x},{y},{yaw}')
        return points
        
    def nav_to_pose(self,target_point):
        self.goToPose(target_point)
        while not self.isTaskComplete():
            feedback = self.getFeedback()
            self.get_logger().info(f'剩余距离:{feedback.distance_remaining}')
        res = self.getResult()
        self.get_logger().info(f'导航结果:{res}')
        
    def get_current_pose(self):
        while rclpy.ok():
            try:
                res = self.buffer.lookup_transform('map', 'base_footprint', rclpy.time.Time(seconds=0.0),rclpy.time.Duration(seconds=1.0))
                transform = res.transform
                self.get_logger().info(f'平移:{transform.translation}')
                return transform
            except Exception as e:
                self.get_logger().warn(f'获取坐标变换失败:原因{str(e)}')
                
    def speech_text(self, text):
        while not self.speech_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('语音合成服务未上线，等待中.....')
        
        request = SpeechText.Request()
        request.text = text
        future = self.speech_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        if future.result() is not None:
            response = future.result()
            if response.result == True:
                self.get_logger().info(f'语音合成成功{text}')
            else:
                self.get_logger().warn(f'语音合成失败{text}')
        else:
            self.get_logger().warn(f'语音合成服务失败')
    
                
def main():
    rclpy.init()
    node = PartolNode()
    node.speech_text('正在准备初始化位置信息')
    node.init_robot_pose()
    node.speech_text('位置信息初始化完成')
    
    while rclpy.ok():
        points = node.get_target_point()
        for point in points:
            x,y,yaw = point[0],point[1],point[2]
            target_point = node.get_pose_by_xyyaw(x,y,yaw)
            node.speech_text(f'正在准备前往{x} {y} 目标点')
            node.nav_to_pose(target_point)
            node.speech_text(f'以及到达目标点{x} {y}，正在记录图像')
            node.record_img()
            node.speech_text('图像记录完成')
    
    rclpy.shutdown()